function [u,info,oMPC] = MPC_Matlab_T_update(iMPC,feedback,x0)
% [x,fval] = MPC1xx_update(feedback,settings,x0,iState)
% General interface of MPC with C. In C code, we always call MPC1xx with
% different ID to specify which MPC we want to use. Different MPCs differ
% in cost function form, upper and lower bound. MPC1xx read from feedback
% and settings and setup constraints for fmincon().
%
% INPUTS:
%    feedback:
%    settings:
%    PridictionHorizon:
%    ControlHorizon:
%    dt:
%    D_set:
%    D_factor:
%    R_set:
%    R_factor:
%    h_set:
%    h_factor:
%    v_set:
%    v_factor:
%    weighting: a vector weights the cost at each steps.
%    Detail:
%    FileNameLog:
% OUTPUTS:
%    u: deposition rate of all the predicted future steps.
%    info:
%      Hopt:
%      Dopt:
%      R2opt:
%      varR2opt:
%      Uopt:
%      cost:
%      exitflag:
%    oState: structure representing updated open-loop model
% AUTHOR: Xinyu Zhang (zxy1256@gmail.com)
% DATE: 2009.08.25

assert(isstruct(feedback),'feedback must be a structure');
assert(all(isfield(feedback,{'alpha','beta','rho','h'})));

assert(isstruct(iMPC),'iMPC must be a structure');
assert(all(isfield(iMPC,{'D_set','Fact_D',...
    'R2_set','Fact_r2',...
    'Ht_set','Fact_H',...
    'P','dt','lb','ub','rt_T'})));

assert(isscalar(x0),'x0 must be a scalar');

oMPC = iMPC;
oMPC.model = model_calstat(iMPC.model,feedback);
P  = iMPC.P;

if oMPC.FeedThrough == 0
    % --------- Linear inequality constraints -----------
    A = [];
    b = [];
    % --------- Linear equality constraints -------------
    Aeq = [];
    beq = [];
    % --------- Upper and lower bound -------------------
    lb = zeros(P,1);
    ub = zeros(P,1);
    for i = 1:P
        lb_temp1 = x0-iMPC.dt*iMPC.rt_T*i;
        lb(i) = max([lb_temp1,iMPC.lb]);
        ub_temp1 = x0+iMPC.dt*iMPC.rt_T*i;
        ub(i) = min([ub_temp1,iMPC.ub]);
    end
    % -------- Initial value ----------------------------
    % Implement the initial value mechanism in C code
    mpc0 = oMPC;
    mpc0.P = 1;
    cost_min = 1e30;
    T_ini = x0(1);
    for T0=lb(1):(ub(1)-lb(1))/100:ub(1)
        cost_temp = objfun_T(T0,feedback,mpc0);
        if(cost_temp<cost_min)
            cost_min = cost_temp;
            T_ini = T0;
        end
    end
    T = T_ini*ones(P,1);
    
    % --------- Solving the optimization problem --------
    option = optimset('LargeScale','off','Display','off');
    [x,fval,exitflag] = fmincon(@(y) objfun_T(y,feedback,oMPC),T,A,b,Aeq,beq,lb,ub,[],option);
    assert(exitflag>=0,'Infeasible solution is encounterd');
    
    % --------- Output ----------------------------------
    % mpc.model = model_calstat(mpc.model,feedback);
    % mpc.model.varAlpha2 = iState.varAlpha2;
    % mpc.model.varBeta2  = iState.varBeta2;
    % mpc.model.varR2     = iState.varR2;
    
    model_pred = oMPC.model;
    info.M2_0 = model_CalMeanM2(model_pred);
    for i = 1:P
        model_pred = model_T_update(model_pred,x(i),oMPC.dt);
        info.Hopt(i)     = model_pred.h;
        info.Dopt(i)     = model_pred.rho;
        info.R2opt(i)    = model_pred.meanR2;
        info.varR2opt(i) = model_pred.varR2;
        info.M2opt(i)    = model_pred.meanM2;
    end
    u = x(1);
    info.Uopt     = x;
    info.cost     = fval;
    info.exitflag = exitflag;
    
    oMPC.model = model_T_update(oMPC.model,x(1),oMPC.dt);
else
    model_pred = oMPC.model;
    info.M2_0 = model_CalMeanM2(model_pred);
    for i = 1:P
        model_pred       = model_T_update(model_pred,x0,oMPC.dt);
        info.Hopt(i)     = model_pred.h;
        info.Dopt(i)     = model_pred.rho;
        info.R2opt(i)    = model_pred.meanR2;
        info.varR2opt(i) = model_pred.varR2;
        info.M2opt(i)    = model_pred.meanM2;
    end
    u = x0;
    info.Uopt = x0*ones(P,1);
    info.cost = 0;
    info.exitflag = 0;
end

end

%%
function F = objfun_T(x,feedback,settings)
% F = objfun(x,feedback,settings)
% F =
%    sum_{i=1}^{i=P}(((H_set-H_i)/H_set)^2+((D_set-D_i)/D_set)^2+((R2_set-R2_i)/R2_set)^2+(varR2_i)^2+((M2_set-M2_i)/M2_set)^2)
%
% var(R^2) is from analytical solution
% mean(R^2) is calculate using analytical solution of alpha^2 and beta^2
%
% SEE ALSO: MPC1xx_b.m

%
% assert(isstruct(settings),'\nsettings should be a struct');
% assert(isstruct(feedback),'\nfeedback should be a struct');
%
H_set       = settings.Ht_set;
H_fact      = settings.Fact_H;
D_set       = settings.D_set;
D_fact      = settings.Fact_D;
R2_set      = settings.R2_set;
R2_fact     = settings.Fact_r2;
varR2_fact  = settings.Fact_varR2;
M2_set      = settings.M2_set;
M2_fact     = settings.M2_fact;
P           = settings.P;
%
% State = model_setState(feedback);

% Because variance cannot be calculated from measurement, the value from
% open loop simulator will be used.
model_pre           = settings.model;
% model_pre.varAlpha2 = iState.varAlpha2;
% model_pre.varBeta2  = iState.varBeta2;
% model_pre.varR2     = iState.varR2;
F = 0;
for i = 1:P
    % Make prediction
    model_pre = model_T_update(model_pre,x(i),settings.dt);
    
    % Calculate cost
    if model_pre.h < H_set
        cost_H = H_fact*((H_set-model_pre.h)/H_set)^2;
    else
        cost_H = 0;
    end
    cost_D      = D_fact*((D_set-model_pre.rho)/D_set)^2;
    cost_meanR2 = R2_fact*((R2_set-model_pre.meanR2)/R2_set)^2;
    cost_varR2  = varR2_fact*(model_pre.varR2)^2;
    cost_meanM2 = M2_fact*((model_pre.meanM2-M2_set)/M2_set)^2;
    F = F+settings.weight(i)*(cost_H+cost_D+cost_meanR2+cost_varR2+cost_meanM2);
end
end
